In [3]:

    

import sympy as sy
sy.init_printing()


    

In [4]:

    

M, m, g, l, u, w, z, theta, thetadot = sy.symbols('M,m,g,l,u,w,z,theta, thetadot')


    

In [5]:

    

Mm = sy.Matrix([[M+m, -m*l*sy.cos(theta)], [-m*l*sy.cos(theta), m*l**2]])
Mm


    

    
Out[5]:





$$\left[\begin{matrix}M + m & - l m \cos{\left (\theta \right )}\\- l m \cos{\left (\theta \right )} & l^{2} m\end{matrix}\right]$$

In [6]:

    

G = sy.Matrix([[m*l*sy.sin(theta)*thetadot**2],[m*g*l*sy.sin(theta)]])
G


    

    
Out[6]:





$$\left[\begin{matrix}l m \dot{\theta}^{2} \sin{\left (\theta \right )}\\g l m \sin{\left (\theta \right )}\end{matrix}\right]$$

In [7]:

    

F = sy.Matrix([[u+w],[l*w]])
F


    

    
Out[7]:





$$\left[\begin{matrix}u + w\\l w\end{matrix}\right]$$

In [8]:

    

acc = Mm.inv()*(F-G)
sy.simplify(acc.subs({w:0}))


    

    
Out[8]:





$$\left[\begin{matrix}- \frac{1}{2 M + 2 m \sin^{2}{\left (\theta \right )}} \left(g m \sin{\left (2 \theta \right )} + 2 l m \dot{\theta}^{2} \sin{\left (\theta \right )} - 2 u\right)\\- \frac{1}{l \left(M + m \sin^{2}{\left (\theta \right )}\right)} \left(g \left(M + m\right) \sin{\left (\theta \right )} + \left(l m \dot{\theta}^{2} \sin{\left (\theta \right )} - u\right) \cos{\left (\theta \right )}\right)\end{matrix}\right]$$

In [9]:

    

sy.simplify(sy.sin(theta+sy.pi))


    

    
Out[9]:





$$- \sin{\left (\theta \right )}$$

In [10]:

    

sy.simplify(sy.cos(theta+sy.pi))


    

    
Out[10]:





$$- \cos{\left (\theta \right )}$$

In [11]:

    

sy.simplify(sy.cos(-theta))


    

    
Out[11]:





$$\cos{\left (\theta \right )}$$

In [12]:

    

theta2 = -theta+sy.pi


    

In [13]:

    

sy.simplify(sy.cos(theta2))


    

    
Out[13]:





$$- \cos{\left (\theta \right )}$$

In [14]:

    

from sympy.physics.mechanics import *
q1,q2 = dynamicsymbols('q1,q2')  # q1=z, q2=theta
q1d,q2d = dynamicsymbols('q1,q2',1)

zpdot = q1d -l*sy.cos(q2)*q2d
ypdot = l*sy.sin(q2)*q2d

T = 0.5*M*q1d**2 + 0.5*m*(zpdot**2 + ypdot**2)
sy.simplify(sy.expand(T))


    

    
Out[14]:





$$0.5 M \left(\frac{d}{d t} \operatorname{q_{1}}{\left (t \right )}\right)^{2} + 0.5 l^{2} m \left(\frac{d}{d t} \operatorname{q_{2}}{\left (t \right )}\right)^{2} - 1.0 l m \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{q_{1}}{\left (t \right )} \frac{d}{d t} \operatorname{q_{2}}{\left (t \right )} + 0.5 m \left(\frac{d}{d t} \operatorname{q_{1}}{\left (t \right )}\right)^{2}$$

In [15]:

    

yp = -l*sy.cos(q2)
U = m*g*yp
L = T-U
L


    

    
Out[15]:





$$0.5 M \left(\frac{d}{d t} \operatorname{q_{1}}{\left (t \right )}\right)^{2} + g l m \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} + 0.5 m \left(l^{2} \sin^{2}{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \left(\frac{d}{d t} \operatorname{q_{2}}{\left (t \right )}\right)^{2} + \left(- l \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \frac{d}{d t} \operatorname{q_{2}}{\left (t \right )} + \frac{d}{d t} \operatorname{q_{1}}{\left (t \right )}\right)^{2}\right)$$

In [16]:

    

LM=LagrangesMethod(L, [q1,q2])


    

In [17]:

    

sy.simplify(LM.form_lagranges_equations())


    

    
Out[17]:





$$\left[\begin{matrix}1.0 M \frac{d^{2}}{d t^{2}}  \operatorname{q_{1}}{\left (t \right )} + m \left(l \sin{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \left(\frac{d}{d t} \operatorname{q_{2}}{\left (t \right )}\right)^{2} - l \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}}  \operatorname{q_{2}}{\left (t \right )} + \frac{d^{2}}{d t^{2}}  \operatorname{q_{1}}{\left (t \right )}\right)\\l m \left(g \sin{\left (\operatorname{q_{2}}{\left (t \right )} \right )} + l \frac{d^{2}}{d t^{2}}  \operatorname{q_{2}}{\left (t \right )} - \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \frac{d^{2}}{d t^{2}}  \operatorname{q_{1}}{\left (t \right )}\right)\end{matrix}\right]$$

In [18]:

    

sy.simplify(LM.mass_matrix)


    

    
Out[18]:





$$\left[\begin{matrix}1.0 M + 1.0 m & - 1.0 l m \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )}\\- 1.0 l m \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} & 1.0 l^{2} m\end{matrix}\right]$$

In [19]:

    

sy.simplify(LM.forcing)


    

    
Out[19]:





$$\left[\begin{matrix}- 1.0 l m \sin{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \left(\frac{d}{d t} \operatorname{q_{2}}{\left (t \right )}\right)^{2}\\- g l m \sin{\left (\operatorname{q_{2}}{\left (t \right )} \right )}\end{matrix}\right]$$

In [20]:

    

sy.simplify(LM.mass_matrix.inv()*LM.forcing)


    

    
Out[20]:





$$\left[\begin{matrix}- \frac{1.0 m \sin{\left (\operatorname{q_{2}}{\left (t \right )} \right )}}{M + m \sin^{2}{\left (\operatorname{q_{2}}{\left (t \right )} \right )}} \left(g \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} + l \left(\frac{d}{d t} \operatorname{q_{2}}{\left (t \right )}\right)^{2}\right)\\- \frac{1.0 \sin{\left (\operatorname{q_{2}}{\left (t \right )} \right )}}{l \left(M + m \sin^{2}{\left (\operatorname{q_{2}}{\left (t \right )} \right )}\right)} \left(g \left(M + m\right) + l m \cos{\left (\operatorname{q_{2}}{\left (t \right )} \right )} \left(\frac{d}{d t} \operatorname{q_{2}}{\left (t \right )}\right)^{2}\right)\end{matrix}\right]$$

In [ ]: